/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "PositionFileReader.hpp"
CPositionFileReader::CPositionFileReader()
  :iFileReaded(false),
   iData(),
   iTimes(),
   iBegin(),
   iEnd(),
   iMaCIGroup(""),
   iInstanceName(""),
   iContainsTimestamp(false),
   iContainsPosition(false),
   iContainsVariance(false),
   iContainsProbability(false),
   iContainsSpeed(false)
{


}


CPositionFileReader::~CPositionFileReader()
{
  
  for(EACH_IN_i(iData)){

    delete(i->second);
   
  }
   iData.clear();
   iTimes.clear();
}

/**
 * Opens file with wanted filename
 */
bool CPositionFileReader::ReadFile(std::string aFileName)
{
  if(iFileReaded){
    for(EACH_IN_i(iData)){
      delete(i->second);
    }
    iData.clear();
    iTimes.clear();
  }
  dPrint(3,"Opening file '%s'",aFileName.c_str());
  FILE *file= fopen(aFileName.c_str(),"rt");
  if(file == NULL){
    dPrint(1,"Cannot open '%s'",aFileName.c_str());
    return false;
  }
	
	
  char c[200];
  char instanceName[64];
  char macigroup[256];
  float version = 0.0;
  //Read the four first lines..and get the version number, macigroup & instancename...

  
  fscanf(file,"%s %s %s %f",c,c,c,&version);

  dPrint(1,"Version number %f",version);
  fscanf(file,"%s %s %s %s",c,macigroup,c,instanceName);
  fgets (c, 200,file );
  fgets (c, 200,file );
  //Look what datatypes are logged
  std::string types(c);
  std::string finalPrint = "Log file contains : ";
  std::string::size_type loc = types.find( "timestamp_sec", 0 );
  if( loc != std::string::npos ) {
    iContainsTimestamp=  true;
    finalPrint +="'timestamp' ";
  }

  loc = types.find("position_x",0);
  if( loc != std::string::npos ) {
    iContainsPosition=  true;
    finalPrint +="'position' ";
  }


  loc = types.find("variance_x",0);
  if( loc != std::string::npos ) {
     iContainsVariance=  true;
     finalPrint +="'variance' ";
  }
 

  loc = types.find("probability",0);
  if( loc != std::string::npos ) {
 
    iContainsProbability=  true;
    finalPrint += "'probability' ";
  }
   dPrint(1,"%s",c);

   loc = types.find("speed",0);
  if( loc != std::string::npos ) {
 
    iContainsSpeed=  true;
    finalPrint += "'speed' ";
  }
  finalPrint += "fields.";



  iMaCIGroup = std::string(macigroup);
  iInstanceName = std::string(instanceName);
  dPrint(1,"Got for macigroup: '%s' and instance name: '%s'",
         iMaCIGroup.c_str(),
         iInstanceName.c_str());
  bool result = false;
  if(version == 0.0){
    result =ReadData(file);
  }else if(version == 2.0){
    result =ReadDataV_2_0(file);
  }
  iFileReaded = result;
  fclose(file);
  return result;
}
//*****************************************************************************

bool CPositionFileReader::ReadData(FILE *aFile)
{
  using namespace MaCI::Position;
  MaCI::Common::TTimestamp timestamp;
  float posX,posY,posA,varX,varY,varA,prob,speed,angularSpeed;
  int number = 1;
  bool first = true;
  if(!iContainsTimestamp){
    dPrint(3,"File doesn't contain timestamps. can't play data without timestamp");
    return false;
  }
  while(number != 0 && number != EOF){
    
    number = fscanf(aFile,"%u %u",
                    &(timestamp.timestamp_s),
                    &(timestamp.timestamp_us));
    if(first && number == 2){
      iBegin = timestamp.GetGimTime();
      first = false;
    }    

    if(iContainsPosition){
      number = fscanf(aFile,"%f %f %f",
                      &posX,&posY,&posA);
                   
      if(number != 3){
       
        continue;
      }
    }
    if(iContainsVariance){
      number = fscanf(aFile,"%f %f %f",
                      &varX,&varY,&varA);
      if(number != 3){
       
        continue;
      }
    }
    if(iContainsProbability){
      number = fscanf(aFile,"%f",
                      &prob);
      if(number != 1){
   
        continue;
      }
    }

    if(iContainsSpeed){
      number = fscanf(aFile,"%f %f",
                      &speed,&angularSpeed);
      if(number != 2){
   
        continue;
      }

    }
    if(number> 0){
      CPositionData *pos = new CPositionData();
      pos->CreateInternalBinBag();
      pos->SetTimestamp(timestamp);
      if(iContainsPosition){
        pos->SetPose2D(TPose2D(posX,posY,posA));
      }
      if(iContainsVariance){
        pos->SetVariance2D(TVariance2D(varX,varY,varA));
      }

      if(iContainsProbability){
        pos->SetProbability(TProbability(prob));
      }
      if(iContainsSpeed){
        pos->SetSpeed2D(TSpeed2D(speed,angularSpeed));
      }
      //pos->Print(1);
      iData[timestamp.GetGimTime()] = pos;
      iTimes.push_back(timestamp.GetGimTime());
    }

    

  }
  if(!first){
    iEnd = timestamp.GetGimTime();
  }
  if(number == EOF){
    dPrint(1,"Stored %d measurements ",iTimes.size());
    return true;
  }else{
    return false;
  }
  
}
//*****************************************************************************

bool CPositionFileReader::ReadDataV_2_0(FILE *aFile)
{

  using namespace MaCI::Position;
  MaCI::Common::TTimestamp timestamp;
  float posX,posY,posZ,posAX,posAY,posAZ,varX,varY,varZ,varAX,varAY,varAZ,prob,speed,angularSpeed;
  int number = 1;
  bool first = true;
  if(!iContainsTimestamp){
    dPrint(3,"File doesn't contain timestamps. can't play data without timestamp");
    return false;
  }
  while(number != 0 && number != EOF){
    
    number = fscanf(aFile,"%u %u",
                    &(timestamp.timestamp_s),
                    &(timestamp.timestamp_us));
    if(first && number == 2){
      iBegin = timestamp.GetGimTime();
      first = false;
    }    

    if(iContainsPosition){
      number = fscanf(aFile,"%f %f %f",
                      &posX,&posY,&posZ);
                   
      if(number != 3){
       
        continue;
      }
    }
    
    if(iContainsPosition){
      number = fscanf(aFile,"%f %f %f",
                      &posAX,&posAY,&posAZ);
                   
      if(number != 3){
       
        continue;
      }
    }
    if(iContainsVariance){
      number = fscanf(aFile,"%f %f %f",
                      &varX,&varY,&varZ);
      if(number != 3){
       
        continue;
      }
    }
    if(iContainsVariance){
      number = fscanf(aFile,"%f %f %f",
                      &varAX,&varAY,&varAZ);
      if(number != 3){
       
        continue;
      }
    }
    if(iContainsProbability){
      number = fscanf(aFile,"%f",
                      &prob);
      if(number != 1){
   
        continue;
      }
    }

    if(iContainsSpeed){
      number = fscanf(aFile,"%f %f",
                      &speed,&angularSpeed);
      if(number != 2){
   
        continue;
      }

    }
    if(number> 0){
      CPositionData *pos = new CPositionData();
      pos->CreateInternalBinBag();
      pos->SetTimestamp(timestamp);
      if(iContainsPosition){
        pos->SetPose3D(TPose3D(posX,posY,posZ,posAX,posAY,posAZ));
      }
      if(iContainsVariance){
        pos->SetVariance3D(TVariance3D(varX,varY,varZ,varAX,varAY,varAZ));
        
      }

      if(iContainsProbability){
        pos->SetProbability(TProbability(prob));
      }
      if(iContainsSpeed){
        pos->SetSpeed2D(TSpeed2D(speed,angularSpeed));
      }
      //pos->Print(1);
      iData[timestamp.GetGimTime()] = pos;
      iTimes.push_back(timestamp.GetGimTime());
    }

    

  }
  if(!first){
    iEnd = timestamp.GetGimTime();
  }
  if(number == EOF){
    dPrint(1,"Stored %d measurements ",iTimes.size());
    return true;
  }else{
    return false;
  }
  
  return false;
}
bool CPositionFileReader::GetServiceInformation(std::string &aMaCIGroup,
                                                std::string &aInstanceName)
{
  if(iFileReaded){
    aMaCIGroup = iMaCIGroup;
    aInstanceName = iInstanceName;
  }else{
    dPrint(1,"file not readed, could not give service information");
  }

  return iFileReaded;


}
//*****************************************************************************

bool CPositionFileReader::GetTimes(std::list<gim::time> &aTimes)
{
  if(iFileReaded){
    aTimes.assign(iTimes.begin(),iTimes.end());
    return true;
  }else{
    dPrint(1,"File not read, could not give times");
    return false;
  }
}
//*****************************************************************************

bool CPositionFileReader::GetTimeLimits(gim::time &aBegin,gim::time &aEnd)
{
   if(iFileReaded){
    aBegin = iBegin;
    aEnd = iEnd;
    return true;
  }else{
     dPrint(1,"File not read, could not give time limits");
    return false;
  }
}
//*****************************************************************************

bool CPositionFileReader::GetAllData(TPositionDataMap &aData)
{
  if(iFileReaded){
    aData.insert(iData.begin(),iData.end());
    return true;
  }else{
    dPrint(1,"File not read, could not give data");
    return false;
  }

}
//*****************************************************************************

bool CPositionFileReader::GetPositionData(MaCI::Position::CPositionData &aData,
                                          gim::time aTime)
{
 if(iFileReaded){
   std::map<gim::time,MaCI::Position::CPositionData *>::iterator iter
     =iData.find(aTime);
   
   if(iter != iData.end()){
     aData = (*(iter->second));
     return true;
   }else{
     dPrint(1,"No data at wanted time  %u %u...",aTime.seconds,aTime.useconds);
     return false;
   }
  }else{
   dPrint(1,"File not read, could not give data");
    return false;
  }
}
//*****************************************************************************
bool CPositionFileReader::getClosestTo(MaCI::Position::CPositionData &aData, gim::time aTime){
	if(!iFileReaded) return false;
	
	///This makes a stupid search ... "Continous" version should also be done 
	std::list<gim::time>::iterator ii;
	double diff, mindiff=1e10;
	gim::time minTime;
	
	///While the time difference is decresing
	for(ii=iTimes.begin();ii!=iTimes.end();ii++){
		diff = fabs((aTime - *ii).getTimeInSeconds());
		//fprintf(stderr,"%lf\n",diff);
		if(diff < mindiff){
			mindiff = diff;
			minTime = *ii;
		}else{
			//fprintf(stderr,"done\n");
			break;
		}
	}
	//if(ii==iTimes.end()){
	//	dPrint(1,"Last item - damn");
	//	return false;
	//	}
	
	std::map<gim::time,MaCI::Position::CPositionData *>::iterator iter
     =iData.find(minTime);
   
   if(iter != iData.end()){
     aData = (*(iter->second));
     return true;
   }else{
     dPrint(1,"No data at wanted time  %u %u...",aTime.seconds,aTime.useconds);
     return false;
   }
	

	return true;
}



//*****************************************************************************

bool CPositionFileReader::GetPositionData(TPositionLogLine &aData,
                                          gim::time aTime)
{
 if(iFileReaded){
   std::map<gim::time,MaCI::Position::CPositionData *>::iterator iter
     =iData.find(aTime);
   
   if(iter != iData.end()){
     const MaCI::Common::TTimestamp *timestamp = iter->second->GetTimestamp();
     const MaCI::Position::TPose2D *pose = iter->second->GetPose2D();
     const MaCI::Position::TSpeed2D *speed = iter->second->GetSpeed2D();
     const MaCI::Position::TVariance2D *var = iter->second->GetVariance2D();
     const MaCI::Position::TProbability *prob =iter->second->GetProbability();
     if(timestamp){
       aData.timestamp_s = timestamp->timestamp_s;
       aData.timestamp_us = timestamp->timestamp_us;
     }else{
       aData.timestamp_s = 0;
       aData.timestamp_us = 0;
     }
     if(pose){
       aData.positionX = pose->x;
       aData.positionY= pose->y;
       aData.heading= pose->a;
     }else{
       aData.positionX = 0.0;
       aData.positionY= 0.0;
       aData.heading=0.0;
     }


     if(var){
       aData.varianceX = var->x;
       aData.varianceY= var->y;
       aData.varianceHeading = var->a;
     }else{
       aData.varianceX = 0.0;
       aData.varianceY= 0.0;
       aData.varianceHeading = 0.0;
     }

     if(prob){
       aData.probability = prob->probability;
     }else{
       aData.probability = 0.0;
     }

     if(speed){
       aData.speed = speed->speed;
       aData.angularSpeed = speed->angularSpeed;
     }else{
       aData.speed = 0.0;
       aData.angularSpeed =0.0;
     }
     return true;
   }else{
     dPrint(1,"No data at wanted time  %u %u...",aTime.seconds,aTime.useconds);
     return false;
   }
  }else{
   dPrint(1,"File not readed :(");
    return false;
  }
}
//*****************************************************************************

bool CPositionFileReader::GetPositionData(MaCI::Position::TPose2D &aData,
                                          gim::time aTime)
{
 if(iFileReaded){
   std::map<gim::time,MaCI::Position::CPositionData *>::iterator iter
     =iData.find(aTime);
   
   if(iter != iData.end()){
     const MaCI::Position::TPose2D *pose = iter->second->GetPose2D();
     if(pose){
       aData = *pose;
       return true;
     }else{
       return false;
     }
   
   }else{
     dPrint(1,"No data at wanted time  %u %u...",aTime.seconds,aTime.useconds);
     return false;
   }
  }else{
    return false;
  }
}
//*****************************************************************************
    
bool CPositionFileReader::GetPositionData(MaCI::Position::TPose3D &aData,
                                          gim::time aTime)
{
 if(iFileReaded){
   std::map<gim::time,MaCI::Position::CPositionData *>::iterator iter
     =iData.find(aTime);
   
   if(iter != iData.end()){
     const MaCI::Position::TPose3D *pose = iter->second->GetPose3D();
     if(pose){
       aData = *pose;
       return true;
     }else{
       return false;
     }
   
   }else{
     dPrint(1,"No data at wanted time  %u %u...",aTime.seconds,aTime.useconds);
     return false;
   }
  }else{
    return false;
  }
}
//*****************************************************************************
                                      
bool CPositionFileReader::GetPositionData(MaCI::Position::TPose2D &aPose,
                                          MaCI::Position::TSpeed2D &aSpeed,
                                          MaCI::Position::TVariance2D &aVariance,
                                          MaCI::Position::TProbability &aProbability,
                                          gim::time aTime)
{
 if(iFileReaded){
   std::map<gim::time,MaCI::Position::CPositionData *>::iterator iter
     =iData.find(aTime);
   
   if(iter != iData.end()){
     const MaCI::Position::TPose2D *pose = iter->second->GetPose2D();
     if(pose){
       aPose = *pose;
     }
     const MaCI::Position::TSpeed2D *speed = iter->second->GetSpeed2D();
     if(speed){
       aSpeed = *speed;
     }
     const MaCI::Position::TVariance2D *var = iter->second->GetVariance2D();
     if(var){
       aVariance = *var;
     }
     const MaCI::Position::TProbability *prob =iter->second->GetProbability();
     if(prob){
       aProbability =*prob;
     }

     if(pose && speed && var &&prob){
       return true;
     }else{
       return false;
     }
   
   }else{
     dPrint(1,"No data at wanted time  %u %u...",aTime.seconds,aTime.useconds);
     return false;
   }
  }else{
    return false;
  }
}

//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
